//
// Created by nsy on 2020/11/20.
//

#include "../../include/shader/common_shader.h"


/**
 * 设置待处理点云
 * @param[input] cloud
 * @return
 */
void CommonShader::setInputCloud(const CommonShader::PointCloudPtr cloud) {
    cloud_ = cloud;
    
}

/**
 * 设置八叉树体素大小
 * @param[input] resolution
 * @return
 */
void CommonShader::setResolution(float resolution) {
    resolution_ = resolution;
    
}

/**
 * 获取八叉树体素大小
 * @return
 */
float CommonShader::getResolution() {
    return resolution_;
}


/**
 * 根据相机位姿信息来进行点云空间变换
 * @param[input] cloud_in 待变换点云
 * @param[output] cloud_out 变换后点云
 * @param[input] camera 对应相机
 * @return
 */
void CommonShader::transformCloudByCamera(CommonShader::PointCloud cloud_in,
                                                     CommonShader::PointCloud &cloud_out,
                                                     CameraInfo camera) {

    Eigen::Affine3d affine(camera.getPose());
    pcl::transformPointCloud(cloud_in, cloud_out, affine);
    
}


/**
 * 检测空间点到摄像机之间有无被其他点遮挡
 *   需要先将点云变换到相机坐标系下
 * @param[input] pt 输入点
 * @param[input] octree 点云八叉树
 * @return 是否被阻挡
 */
bool CommonShader::isPointOccluded(const Point &p, OctreePtr octree) {
    Eigen::Vector3f direction;
    direction(0) = p.x;
    direction(1) = p.y;
    direction(2) = p.z;

    std::vector<int> indices;
    PointCloudConstPtr cloud_octree(new PointCloud);
    cloud_octree = octree->getInputCloud();

    /// z方向阈值设置为八叉树体素大小×10
    double distance_threshold = octree->getResolution() * 10;

    // 利用八叉树进行射线检测
    octree->getIntersectedVoxelIndices(direction * 0, direction, indices);
    int nbocc = static_cast<int>(indices.size());
    for (size_t j = 0; j < indices.size(); j++) {
        // 点在该点后面或不同方向
        if (p.z * cloud_octree->points[indices[j]].z < 0 || p.z <= cloud_octree->points[indices[j]].z) {
            nbocc--;
            continue;
        }
        if (fabs(cloud_octree->points[indices[j]].z - p.z) <= distance_threshold) {
            // 如果两个点十分接近，则认为在同一平面，忽略影响
            /// 接近被定义为10个体素大小
            nbocc--;
        }
    }

    if (nbocc <= 0)
        return false;
    else
        return true;
}


/**
  * 根据空间点和相机信息获取平面坐标(x,y)
  * 图像左上为(0,0),横轴为x，纵轴为y
  * 0 <= x < 图像宽度  0 <= y < 图像高度 为整数
  * @param[input] p
  * @param[input] camera
  * @param[output] XY_coordinates 平面坐标，不在视角内时为(-1,-1)
  * @return
  */
void CommonShader::getXYCoordinates(const CommonShader::Point &p, CameraInfo camera,
                                               Eigen::Vector2i &XY_coordinates) {

    // 获取相机内参矩阵
    Eigen::Matrix3d parameter = camera.getParamMatrix();

    Eigen::Vector3d v(p.x,p.y,p.z);
    Eigen::Vector3d res;

    // 三维坐标映射到二维平面
    res = parameter * v / v.z();

    if(v.z() < 0 || res.x() < 0 || res.x() >= camera.getWidth() || res.y() < 0 || res.y() >= camera.getWidth()) {
        // 此时该点不在相机视角内
        XY_coordinates.x() = -1;
        XY_coordinates.y() = -1;
    }
    else {
        XY_coordinates.x() = res.x();
        XY_coordinates.y() = res.y();
    }

    
}

/**
 * 根据空间点和相机信息获取平面坐标(x,y)以及点到相机距离
 * 图像左上为(0,0),横轴为x，纵轴为y
 * 0 <= x < 图像宽度  0 <= y < 图像高度 为整数
 * 为减少计算，距离采用曼哈顿距离并非欧氏距离
 * @param[input] p
 * @param[input] camera
 * @param[output] XY_coordinates 平面坐标，不在视角内时为(-1,-1)
 * @param[output] distance 点到相机距离，不在视角内时为MAX_DOUBLE
 * @return
 */
void CommonShader::getXYCoordinates(const CommonShader::Point &p, CameraInfo camera,
                                               Eigen::Vector2i &XY_coordinates, float &distance) {

    getXYCoordinates(p, camera, XY_coordinates);

    if (XY_coordinates.x() < 0) {
        // 不在相机视角内，距离设置为最大
        distance = __FLT_MAX__;
    }
    else {
        // 在相机视角内，设置距离为xyz坐标之和
        distance = fabsf(p.x) + fabsf(p.y) + fabsf(p.z);
    }

    
}

